#include "Until.h"

//the colors we will be using
const U16 CamColors[] = {0x480F,
                         0x400F, 0x400F, 0x400F, 0x4010, 0x3810, 0x3810, 0x3810, 0x3810, 0x3010, 0x3010,
                         0x3010, 0x2810, 0x2810, 0x2810, 0x2810, 0x2010, 0x2010, 0x2010, 0x1810, 0x1810,
                         0x1811, 0x1811, 0x1011, 0x1011, 0x1011, 0x0811, 0x0811, 0x0811, 0x0011, 0x0011,
                         0x0011, 0x0011, 0x0011, 0x0031, 0x0031, 0x0051, 0x0072, 0x0072, 0x0092, 0x00B2,
                         0x00B2, 0x00D2, 0x00F2, 0x00F2, 0x0112, 0x0132, 0x0152, 0x0152, 0x0172, 0x0192,
                         0x0192, 0x01B2, 0x01D2, 0x01F3, 0x01F3, 0x0213, 0x0233, 0x0253, 0x0253, 0x0273,
                         0x0293, 0x02B3, 0x02D3, 0x02D3, 0x02F3, 0x0313, 0x0333, 0x0333, 0x0353, 0x0373,
                         0x0394, 0x03B4, 0x03D4, 0x03D4, 0x03F4, 0x0414, 0x0434, 0x0454, 0x0474, 0x0474,
                         0x0494, 0x04B4, 0x04D4, 0x04F4, 0x0514, 0x0534, 0x0534, 0x0554, 0x0554, 0x0574,
                         0x0574, 0x0573, 0x0573, 0x0573, 0x0572, 0x0572, 0x0572, 0x0571, 0x0591, 0x0591,
                         0x0590, 0x0590, 0x058F, 0x058F, 0x058F, 0x058E, 0x05AE, 0x05AE, 0x05AD, 0x05AD,
                         0x05AD, 0x05AC, 0x05AC, 0x05AB, 0x05CB, 0x05CB, 0x05CA, 0x05CA, 0x05CA, 0x05C9,
                         0x05C9, 0x05C8, 0x05E8, 0x05E8, 0x05E7, 0x05E7, 0x05E6, 0x05E6, 0x05E6, 0x05E5,
                         0x05E5, 0x0604, 0x0604, 0x0604, 0x0603, 0x0603, 0x0602, 0x0602, 0x0601, 0x0621,
                         0x0621, 0x0620, 0x0620, 0x0620, 0x0620, 0x0E20, 0x0E20, 0x0E40, 0x1640, 0x1640,
                         0x1E40, 0x1E40, 0x2640, 0x2640, 0x2E40, 0x2E60, 0x3660, 0x3660, 0x3E60, 0x3E60,
                         0x3E60, 0x4660, 0x4660, 0x4E60, 0x4E80, 0x5680, 0x5680, 0x5E80, 0x5E80, 0x6680,
                         0x6680, 0x6E80, 0x6EA0, 0x76A0, 0x76A0, 0x7EA0, 0x7EA0, 0x86A0, 0x86A0, 0x8EA0,
                         0x8EC0, 0x96C0, 0x96C0, 0x9EC0, 0x9EC0, 0xA6C0, 0xAEC0, 0xAEC0, 0xB6E0, 0xB6E0,
                         0xBEE0, 0xBEE0, 0xC6E0, 0xC6E0, 0xCEE0, 0xCEE0, 0xD6E0, 0xD700, 0xDF00, 0xDEE0,
                         0xDEC0, 0xDEA0, 0xDE80, 0xDE80, 0xE660, 0xE640, 0xE620, 0xE600, 0xE5E0, 0xE5C0,
                         0xE5A0, 0xE580, 0xE560, 0xE540, 0xE520, 0xE500, 0xE4E0, 0xE4C0, 0xE4A0, 0xE480,
                         0xE460, 0xEC40, 0xEC20, 0xEC00, 0xEBE0, 0xEBC0, 0xEBA0, 0xEB80, 0xEB60, 0xEB40,
                         0xEB20, 0xEB00, 0xEAE0, 0xEAC0, 0xEAA0, 0xEA80, 0xEA60, 0xEA40, 0xF220, 0xF200,
                         0xF1E0, 0xF1C0, 0xF1A0, 0xF180, 0xF160, 0xF140, 0xF100, 0xF0E0, 0xF0C0, 0xF0A0,
                         0xF080, 0xF060, 0xF040, 0xF020, 0xF800,
                        };

U16 GetColors(U8 Index)
{
    return CamColors[Index];
}

S32 Min(S32 A, S32 B)
{
    if(A > B)
        return B;

    return A;
}

Void FindMaxTemp(Float *Min, Float *Max, Float *Array)
{
    *Min = *Max = Array[0];

    for(U8 i = 1; i < 64; i++)
    {
        if(Array[i] > *Max) 
            *Max = Array[i];

        if(Array[i] < *Min) 
            *Min = Array[i];
    }
}

Float SignedMag12ToFloat(U16 Val)
{
    U16 AbsVal = (Val & 0x7FF);
    return (Val & 0x8000) ? 0 - (Float)AbsVal : (Float)AbsVal ;
}

U8 Map(Float Val, U8 InMin, U8 InMax, U8 OutMin, U8 OutMax)
{
    return (Val - InMin) * (OutMax - OutMin) / (InMax - InMin) + OutMin;
}

U8 ConStrain(U8 Amt, U8 Low, U8 High)
{
    if(Amt < Low)
        return Low;

    if(Amt > High)
        return High;

    return Amt;
}


Void Reverse(Float *p, S32 Size)
{
    S32 i;
    Float Tmp;

    for(i = 0 ; i < Size / 2 ; i++)
    {
        Tmp = p[i] ;
        p[i] = p[Size - 1 - i] ;
        p[Size - 1 - i] = Tmp ;
    }
}


static Void _TurnRight(Float ar[8][8])
{
    Float br[8][8];
    S32 x, y;

    for (y = 0; y < 8; y++)
    {
        for (x = 0; x < 8; x++)
        {
            br[y][x] = ar[8 - 1 - x][y];
        }
    }

    for (y = 0; y < 8; y++)
    {
        for (x = 0; x < 8; x++)
        {
            ar[y][x] = br[y][x];
        }
    }
}

Void TurnRight(Float *Arr)
{
    S32 Index = 0;
    Float DoubleArr[8][8];

    for(int i = 0; i < 8; i++)
    {
        for(int j = 0; j < 8; j++)
        {
            DoubleArr[i][j] = Arr[i * 8 + j];
        }
    }

    _TurnRight(DoubleArr);

    for(int i = 0; i < 8; i++)
    {
        for(int j = 0; j < 8; j++)
        {
            Arr[Index] = DoubleArr[i][j];
            Index++;
        }
    }
}
